﻿from pb.ros.geometry_msgs import Transform_pb2
from pb.ros.geometry_msgs import Pose_pb2
from pb.ros.custom_msgs import CustomTransform_pb2
from pb.ros.custom_msgs import Float32Array_pb2
from pb.ros.data_control_msgs import Collector_new_pb2
from pb.ros.std_msgs import Bool_pb2
import proxy
import os
import time
import socket
import struct
from ctypes import *
import math
import threading
import sys

# import matlab.engine
# node = proxy.Connector('tcp://12.12.12.204:15555')

node = proxy.Connector('tcp://172.22.56.12:15555') #在709的电脑上使用该IP

publish_restart = node.Advertise('FK', 'System', '/FK/reset', Bool_pb2.Bool)
publish_pause = node.Advertise('FK', 'System', '/FK/pause', Bool_pb2.Bool)
publish_stepforward = node.Advertise('FK', 'System', '/FK/resume', Bool_pb2.Bool)


class Publish:
    def restart(self):
        abool = Bool_pb2.Bool()
        abool.data = True
        publish_restart.Publish(abool)

    def pause(self):
        bbool = Bool_pb2.Bool()
        bbool.data = True
        publish_pause.Publish(bbool)

    def step(self):
        dbool = Bool_pb2.Bool()
        dbool.data = True
        publish_stepforward.Publish(dbool)

    def pub(self):
        # self.restart()
        # self.pause()
        self.step()
        # time.sleep(0.1)


def main(args):
    publish = Publish()
    thread_pub = threading.Thread(target=publish.pub)
    thread_pub.start()

    n = input('')


if __name__ == '__main__':
    main(sys.argv)
